﻿#include "stdafx.h"
#include "Functions.h"

void CoordMultMatrix(float T[3] , float R[3] , float Sou_Matrix[16] , float* Des_Matrix)
{
	float Buff_Matrix[16];
	for (int i =0 ; i < 16 ; i++)
	{
		Buff_Matrix[i] = Sou_Matrix[i];
	}

	float ctx = System::Math::Cos(R[0]);
	float cty = System::Math::Cos(R[1]);
	float ctz = System::Math::Cos(R[2]);
	float stx = System::Math::Sin(R[0]);
	float sty = System::Math::Sin(R[1]);
	float stz = System::Math::Sin(R[2]);

	GLfloat matrix[16];

	matrix[0]=ctz*cty;
	matrix[1]=stz*cty;
	matrix[2]=-sty;
	matrix[3]=0;
	matrix[4]=ctz*sty*stx - stz*ctx;
	matrix[5]=stz*sty*stx + ctz*ctx;
	matrix[6]=cty*stx;
	matrix[7]=0;
	matrix[8]=ctz*sty*ctx + stz*stx;
	matrix[9]=stz*sty*ctx - ctz*stx;
	matrix[10]=cty*ctx;
	matrix[11]=0;
	matrix[12]=T[0];
	matrix[13]=T[1];
	matrix[14]=T[2];
	matrix[15]=1;

	Des_Matrix[0] = Buff_Matrix[0]*matrix[0] + Buff_Matrix[4]*matrix[1] + Buff_Matrix[8]*matrix[2] + Buff_Matrix[12]*matrix[3];
	Des_Matrix[1] = Buff_Matrix[1]*matrix[0] + Buff_Matrix[5]*matrix[1] + Buff_Matrix[9]*matrix[2] + Buff_Matrix[13]*matrix[3];
	Des_Matrix[2] = Buff_Matrix[2]*matrix[0] + Buff_Matrix[6]*matrix[1] + Buff_Matrix[10]*matrix[2] + Buff_Matrix[14]*matrix[3];
	Des_Matrix[3] = Buff_Matrix[3]*matrix[0] + Buff_Matrix[7]*matrix[1] + Buff_Matrix[11]*matrix[2] + Buff_Matrix[15]*matrix[3];

	Des_Matrix[4] = Buff_Matrix[0]*matrix[4] + Buff_Matrix[4]*matrix[5] + Buff_Matrix[8]*matrix[6] + Buff_Matrix[12]*matrix[7];
	Des_Matrix[5] = Buff_Matrix[1]*matrix[4] + Buff_Matrix[5]*matrix[5] + Buff_Matrix[9]*matrix[6] + Buff_Matrix[13]*matrix[7];
	Des_Matrix[6] = Buff_Matrix[2]*matrix[4] + Buff_Matrix[6]*matrix[5] + Buff_Matrix[10]*matrix[6] + Buff_Matrix[14]*matrix[7];
	Des_Matrix[7] = Buff_Matrix[3]*matrix[4] + Buff_Matrix[7]*matrix[5] + Buff_Matrix[11]*matrix[6] + Buff_Matrix[15]*matrix[7];

	Des_Matrix[8] = Buff_Matrix[0]*matrix[8] + Buff_Matrix[4]*matrix[9] + Buff_Matrix[8]*matrix[10] + Buff_Matrix[12]*matrix[11];
	Des_Matrix[9] = Buff_Matrix[1]*matrix[8] + Buff_Matrix[5]*matrix[9] + Buff_Matrix[9]*matrix[10] + Buff_Matrix[13]*matrix[11];
	Des_Matrix[10] = Buff_Matrix[2]*matrix[8] + Buff_Matrix[6]*matrix[9] + Buff_Matrix[10]*matrix[10] + Buff_Matrix[14]*matrix[11];
	Des_Matrix[11] = Buff_Matrix[3]*matrix[8] + Buff_Matrix[7]*matrix[9] + Buff_Matrix[11]*matrix[10] + Buff_Matrix[15]*matrix[11];

	Des_Matrix[12] = Buff_Matrix[0]*matrix[12] + Buff_Matrix[4]*matrix[13] + Buff_Matrix[8]*matrix[14] + Buff_Matrix[12]*matrix[15];
	Des_Matrix[13] = Buff_Matrix[1]*matrix[12] + Buff_Matrix[5]*matrix[13] + Buff_Matrix[9]*matrix[14] + Buff_Matrix[13]*matrix[15];
	Des_Matrix[14] = Buff_Matrix[2]*matrix[12] + Buff_Matrix[6]*matrix[13] + Buff_Matrix[10]*matrix[14] + Buff_Matrix[14]*matrix[15];
	Des_Matrix[15] = Buff_Matrix[3]*matrix[12] + Buff_Matrix[7]*matrix[13] + Buff_Matrix[11]*matrix[14] + Buff_Matrix[15]*matrix[15];
}

void CoordTransPos(float T[3] , float R[3] ,float CoordMatrix[16], float Sou_pos[3] , float* Des_pos)
{

	float ctx = System::Math::Cos(R[0]);
	float cty = System::Math::Cos(R[1]);
	float ctz = System::Math::Cos(R[2]);
	float stx = System::Math::Sin(R[0]);
	float sty = System::Math::Sin(R[1]);
	float stz = System::Math::Sin(R[2]);

	GLfloat matrix[16];

	matrix[0]=ctz*cty;
	matrix[1]=stz*cty;
	matrix[2]=-sty;
	matrix[3]=0;
	matrix[4]=ctz*sty*stx - stz*ctx;
	matrix[5]=stz*sty*stx + ctz*ctx;
	matrix[6]=cty*stx;
	matrix[7]=0;
	matrix[8]=ctz*sty*ctx + stz*stx;
	matrix[9]=stz*sty*ctx - ctz*stx;
	matrix[10]=cty*ctx;
	matrix[11]=0;
	matrix[12]=T[0];
	matrix[13]=T[1];
	matrix[14]=T[2];
	matrix[15]=1;

	GLfloat temp_pos[3];

	temp_pos[0] = matrix[0]*Sou_pos[0] + matrix[4]*Sou_pos[1] + matrix[8]*Sou_pos[2] + matrix[12];
	temp_pos[1] = matrix[1]*Sou_pos[0] + matrix[5]*Sou_pos[1] + matrix[9]*Sou_pos[2] + matrix[13];
	temp_pos[2] = matrix[2]*Sou_pos[0] + matrix[6]*Sou_pos[1] + matrix[10]*Sou_pos[2] + matrix[14];

	Des_pos[0] = CoordMatrix[0]*temp_pos[0] + CoordMatrix[4]*temp_pos[1] + CoordMatrix[8]*temp_pos[2] + CoordMatrix[12];
	Des_pos[1] = CoordMatrix[1]*temp_pos[0] + CoordMatrix[5]*temp_pos[1] + CoordMatrix[9]*temp_pos[2] + CoordMatrix[13];
	Des_pos[2] = CoordMatrix[2]*temp_pos[0] + CoordMatrix[6]*temp_pos[1] + CoordMatrix[10]*temp_pos[2] + CoordMatrix[14];
}

void SetRotateMatrix(float angle_degree, int axis, float* des_matrix)
{
	float angle_rad = angle_degree * System::Math::PI / 180;

	for(int i=0; i<16; i++)
	{
		if((i%5) == 0)
		{
			des_matrix[i] = 1; 
		}
		else
		{
			des_matrix[i] = 0; 
		}
	}

	if(axis == 0) // Rotate X-axis
	{
		des_matrix[5] = System::Math::Cos(angle_rad);
		des_matrix[6] = -System::Math::Sin(angle_rad);
		des_matrix[9] = System::Math::Sin(angle_rad);
		des_matrix[10] = System::Math::Cos(angle_rad);
	}
	else if(axis == 1) // Rotate Y-axis
	{
		des_matrix[0] = System::Math::Cos(angle_rad);
		des_matrix[2] = System::Math::Sin(angle_rad);
		des_matrix[8] = -System::Math::Sin(angle_rad);
		des_matrix[10] = System::Math::Cos(angle_rad);
	}
	else if(axis == 2) // Rotate Z-axis
	{
		des_matrix[0] = System::Math::Cos(angle_rad);
		des_matrix[1] = -System::Math::Sin(angle_rad);
		des_matrix[4] = System::Math::Sin(angle_rad);
		des_matrix[5] = System::Math::Cos(angle_rad);
	}
}

void MetrixTranspose(int RowNum, int ColNum, float* A, float* B)
{
    // B = Transpose(A)
    int i, j;
    int temp1, temp2;
    for(i=0; i < RowNum; i++)
	{
		for(j=0; j < ColNum; j++)
		{
			temp1 = i * ColNum + j;
			temp2 = j * RowNum + i;
			B[temp2] = A[temp1];
		}
	}
}

void MetrixMultiplication(int RowA, int ColA, int RowB, int ColB, float* A, float* B, float* C)
{
	// C = A * B
	if((ColA == RowB) && (RowA > 0) && (RowB > 0) && (ColB > 0))
	{
		int index = 0;
		for(int i=0; i < RowA; i++)
		{
			for(int j=0; j < ColB; j++)
			{
				index = i * ColB + j;
				C[index] = 0;
				for(int k=0; k < RowA; k++)
				{
					C[index] += A[(i*ColA)+k] * B[(k*ColB)+j];
				}
			}
		}
		
	}
}

void AssignArray(int Length, float* A, float* B)
{
	for(int i=0; i<Length; i++)
	{
		B[i] = A[i];
	}
}

void QuaternionRotateVector(float angle_degree, float* rotated_vector, float* base_vector, float* des_vector)
{
	float QR[9] = {0};
	float w = System::Math::Cos((angle_degree / 2) * System::Math::PI / 180);
	float x, y, z;

	NormalizeVector(3, base_vector);
	x = base_vector[0];
	y = base_vector[1];
	z = base_vector[2];
	
	QR[0] = 1 - 2 * (y*y + z*z);
	QR[1] = 2 * (x*y - w*z);
	QR[2] = 2 * (w*y + x*z);
	QR[3] = 2 * (x*y + w*z);
	QR[4] = 1 - 2 * (x*x + z*z);
	QR[5] = 2 * (y*z - w*x);
	QR[6] = 2 * (x*z - w*y);
	QR[7] = 2 * (y*z + w*x);
	QR[8] = 1 - 2 * (x*x + y*y);

	MetrixMultiplication(3, 3, 3, 1, &QR[0], &rotated_vector[0], &des_vector[0]);
}

void QuaternionRotateMetrix(float angle_degree, float* rotated_metrix, float* base_vector, float* des_metrix)
{
	float vector1[3] = {0};
	float vector2[3] = {0};
	float vector3[3] = {0};
	float des_vector1[3] = {0};
	float des_vector2[3] = {0};
	float des_vector3[3] = {0};

	//vector1[0] = rotated_metrix[0];
	//vector1[1] = rotated_metrix[4];
	//vector1[2] = rotated_metrix[8];
	//vector2[0] = rotated_metrix[1];
	//vector2[1] = rotated_metrix[5];
	//vector2[2] = rotated_metrix[9];
	//vector3[0] = rotated_metrix[2];
	//vector3[1] = rotated_metrix[6];
	//vector3[2] = rotated_metrix[10];
	vector1[0] = rotated_metrix[0];
	vector1[1] = rotated_metrix[1];
	vector1[2] = rotated_metrix[2];
	vector2[0] = rotated_metrix[4];
	vector2[1] = rotated_metrix[5];
	vector2[2] = rotated_metrix[6];
	vector3[0] = rotated_metrix[8];
	vector3[1] = rotated_metrix[9];
	vector3[2] = rotated_metrix[10];

	QuaternionRotateVector(angle_degree, &vector1[0], base_vector, &des_vector1[0]);
	QuaternionRotateVector(angle_degree, &vector2[0], base_vector, &des_vector2[0]);
	QuaternionRotateVector(angle_degree, &vector3[0], base_vector, &des_vector3[0]);

	//rotated_metrix[0] = des_vector1[0];
	//rotated_metrix[4] = des_vector1[1];
	//rotated_metrix[8] = des_vector1[2];
	//rotated_metrix[1] = des_vector2[0];
	//rotated_metrix[5] = des_vector2[1];
	//rotated_metrix[9] = des_vector2[2];
	//rotated_metrix[2] = des_vector3[0];
	//rotated_metrix[6] = des_vector3[1];
	//rotated_metrix[10] = des_vector3[2];
	rotated_metrix[0] = des_vector1[0];
	rotated_metrix[1] = des_vector1[1];
	rotated_metrix[2] = des_vector1[2];
	rotated_metrix[4] = des_vector2[0];
	rotated_metrix[5] = des_vector2[1];
	rotated_metrix[6] = des_vector2[2];
	rotated_metrix[8] = des_vector3[0];
	rotated_metrix[9] = des_vector3[1];
	rotated_metrix[10] = des_vector3[2];
}

void NormalizeVector(int length, float* v)
{
	if(length > 0)
	{
		float norm = 0;
		for(int i=0; i<length; i++)
		{
			norm += v[i]*v[i];
		}

		if(norm > 0)
		{
			norm = sqrt(norm);
			for(int i=0; i<length; i++)
			{
				v[i] = v[i] / norm;
			}
		}
	}
}

